#ifndef _CAMERA_CAL_GEOMETRY_HH_
#define _CAMERA_CAL_GEOMETRY_HH_
#include "PinholeCameraC.h"
//	using namespace RavlN;
bool GetFundamentalMat(CvMat* fundamental_mat,CalObservation2dArrayC& obsi,CalObservation2dArrayC& obsj);
void FundamentalMatrixEstimate(Matrix3dC& F, 
							   const CalObservation2dArrayC& pt0,
							   const CalObservation2dArrayC& pt1,
							   bool force_rank_two=true);
//: Compute fundamental matrix
// Uses extended eight-points algorithm, requiring 8 or more point correspondences from two views

bool FundamentalMatrixToEssential(Matrix3dC& E, 
								  const Matrix3dC& F, 
								  const Matrix3dC& K0, 
								  const Matrix3dC& K1);
//: Compute essential matrix from fundamental matrix
//  Assumes known internal camera parameters

bool FundamentalMatrixToEssential(Matrix3dC& E,
								  Matrix3dC& K0,
								  Matrix3dC& K1,
								  const Matrix3dC& F,
								  const Vector2dC& c0,
								  const Vector2dC& c1,
								  RealT sy0,
								  RealT sy1);
//: Compute essential matrix and focal lengths from fundamental matrix
// Requires known principal points and aspect ratios, and assumes rectilinear pixel array

void EssentialMatrixDecompose(Matrix3dC& R, Vector3dC& t, 
							  const Matrix3dC& E,
							  const Matrix3dC& K0,
							  const Matrix3dC& K1,
							  const CalObservation2dC& obs0,
							  const CalObservation2dC& obs1);
//: Decompose essential matrix into rotation and translation
// Requires internal camera parameters and one point correspondence


void PointFindGeometric(Vector3dC& x,
						const PinholeArrayC& camera,
						const SArray1dC<CalObservation2dC>& pt2D);
//: Estimate point location using linear method

void PointFindOptimise(Vector3dC& x,
					   const PinholeArrayC& camera,
					   const SArray1dC<CalObservation2dC>& pt2D,
					   RealT& rErrorSqr);
//: Refine estimate of point location using non-linear method
// NOTE: x must be initialised using PointFindGeometric first.

bool SelfCalibrateRANSAC(Matrix3dC& E, 
						 Matrix3dC& K0,
						 Matrix3dC& K1,
						 CalObservation2dArrayC& obs0,
						 CalObservation2dArrayC& obs1,
						 const Vector2dC& c0,
						 const Vector2dC& c1,
						 RealT sy0,
						 RealT sy1);
bool SelfCalibrateRANSACOPNECV(Matrix3dC& E, 
							   Matrix3dC& K0,
							   Matrix3dC& K1,
							   CalObservation2dArrayC& obs0,
							   CalObservation2dArrayC& obs1,
							   const Vector2dC& c0,
							   const Vector2dC& c1,
							   RealT sy0,
							   RealT sy1);
#endif